#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError

class CameraNode(Node):
    def __init__(self, name):
        super().__init__(name)
        self.image_pub = self.create_publisher(Image, 'camera/image', 10)
        self.cap = cv2.VideoCapture(0)  # 0通常是第一个摄像头设备，根据你的设备号调整
        self.bridge = CvBridge()
        self.timer = self.create_timer(0.1, self.publish_image)
        self.camera_info_msg = CameraInfo()  # 初始化CameraInfo消息，根据需要填写详细信息，例如焦距等。
        self.camera_info_pub = self.create_publisher(CameraInfo, 'camera/camera_info', 10)
        self.camera_info_timer = self.create_timer(1.0, self.publish_camera_info)  # 每秒发布一次CameraInfo消息。
    
    def publish_image(self):
        ret, frame = self.cap.read()
        if ret:
            try:
                msg = self.bridge.cv2_to_imgmsg(frame, "bgr8")
                msg.header.stamp = self.get_clock().now().to_msg()
                msg.header.frame_id = "camera_link"  # 根据需要设置frame_id
                self.image_pub.publish(msg)
            except CvBridgeError as e:
                print(e)
    
    def publish_camera_info(self):
        self.camera_info_msg.header.stamp = self.get_clock().now().to_msg()
        self.camera_info_msg.header.frame_id = "camera_link"  # 根据需要设置frame_id
        # 设置CameraInfo的详细信息，例如焦距、宽度、高度等。这里仅为示例：
        self.camera_info_msg.width = 640  # 示例宽度，根据你的摄像头调整
        self.camera_info_msg.height = 480  # 示例高度，根据你的摄像头调整
        self.camera_info_pub.publish(self.camera_info_msg)
        print("Camera info published")
    
def main(args=None):
    rclpy.init(args=args)
    pub_CameraNode = CameraNode("Camera_Node") #创建Topic_Pub类对象，传入的参数就是节点的名字
    rclpy.spin(pub_CameraNode)     
    #执行rclpy.spin函数，里边传入一个参数，参数是刚才创建好的1
    # Topic_Pub类对象
    pub_CameraNode.destroy_node()  #销毁节点对象
    rclpy.shutdown()         
    #关闭ROS2 Python接口
